﻿#pragma once
#include <string>
#include <vector>
#include <QCoreApplication>
#include <QDir>
#include <QThread>
#include <fstream>
#include <unistd.h>

//中间层会用到各个机械臂和算法接口，所以包含用到的相应头文件
#include "SurgicalRobotCommon.h"
//#include "../hardware_interface/Robot/RobotControlInterface.h"
#include "../hardware_interface/Robot/RobotControlInterface.h"

//#include "../hardware_interface/EndTool/EndToolInterfaceV2.h"
#include "../hardware_interface/EndTool/EndToolInterface.h"
#include "../hardware_interface/EndTool/camera/EndToolCamera.h"
#include "../hardware_interface/EndTool/3dof/EndTool3Dof.h"
#include "../hardware_interface/EndTool/4dof/EndTool4Dof.h"
#include "../hardware_interface/EndTool/BoplarHook/EndToolBoplarHook.h"
#include "../hardware_interface/EndTool/forcep/EndToolForcep.h"


//#include "../hardware_interface/HapticDevice/HapticDeviceControl.h"
#include "../hardware_interface/HapticDevice/HapticDeviceControl.h"

#include "../Algirithm/genera/GeneraAlgirithm.h"
#include "../Algirithm/camera/CameraAlgirithm.h"
#include "../Algirithm/boplarHook/BoplarHookAlgirithm.h"
#include "../Algirithm/3dof/ThreeDofAlgirithm.h"
#include "../Algirithm/4dof/FourDofAlgirithm.h"


//这个是机器人管理器类，被所有上层界面类可以全局调用唯一实例
//SurgicalRobotManager* SRManager = SurgicalRobotManager::GetInstance();
//调用对象：
//SurgicalRobotRCM.h
//SurgicalRobotConfig.h
//SurgicalRobotPlatform.h

//机械臂的ip、端口等配置参数
struct SRobotConfig
{
    char cRobotArmIP[256];//机械臂连接IP
    char cRobotArmGW[256];//机械臂连接端口号
    char cRobotArmMA[256];//机械臂连接网关
    int iRobotarmType;    //机械臂类型
    int iRobotToolType;   //机械臂所持手术器械类型
    char cCoordinateSystem[128];
    double dRCMPosition[3];//RCM点在全局坐标系下的x,y,z
    int iMotornumber[5];
};

class  SurgicalRobotManager :public QThread
{
    Q_OBJECT

public:

    SurgicalRobotManager();
    ~SurgicalRobotManager();
    static SurgicalRobotManager* m_pInstance;

public:
    static  SurgicalRobotManager* GetInstance()
    {
        if (m_pInstance == NULL) {
            m_pInstance = new SurgicalRobotManager();
        }
        return m_pInstance;
    }

    bool bAppExit = false;
    bool IfInited =false;
    int RunCount=0;
    double testcount=0;

    std::string paramsPath = QDir::toNativeSeparators(qApp->applicationDirPath() + "/experiment_data").toStdString();//../../
    std::string robotArmConfigFile = "/robotConfigParam.txt";

    //机械臂配置数据(每一台机械臂的IP,配的手持器械，RCM点坐标)文件导入导出
    SRobotConfig sRobotConfigPara[MaxRobotNum];

    //RobotControlInterface* robotControl[MaxRobotNum];
    RobotControlInterfaceV2* robotControl[MaxRobotNum];

    EndToolInterface* endTool[MaxRobotNum];
    //EndToolInterfaceV2* endTool[MaxRobotNum];

    //HapticDeviceControl HDControl[2];
    HapticDeviceControlV2* HDControl[2];


public:
    //Algirithm
    GeneraAlgirithm* pGeneraAlgirithm = new GeneraAlgirithm();
    CameraAlgirithm* pCameraAlgirithm = new CameraAlgirithm();
    BoplarHookAlgirithm* pBoplarHookAlgirithm = new BoplarHookAlgirithm();
    ThreeDofAlgirithm* pThreeDofAlgirithm = new ThreeDofAlgirithm();
    FourDofAlgirithm* pFourDofAlgirithm = new FourDofAlgirithm();

public:
    //experimental data
    std::fstream  read_robot_experimrnt;

public:
    //上层UI调用
    void InitConfigParam();
    void SaveRobotConfig();
    void ReadConfigParam();

    void initSegmaSimple(int instanceindex,int segmaindex);

    void initRobotSimple(int robotNumber,int robotType, QString IP);
    void initEndToolSimple(int ToolNumber,int ToolType);

    bool initRobot(int robotNumber, const char* ipAddr);
    bool initEndTool(int robotNumber);


    //非实时交互方法：
    void MoveJ(int robotid,int toolid,LREALArray10 RobotJointPos,LREALArray10 EndToolJointPos);//例如回零点，准备点
    void MoveL(int robotid,int toolid,Eigen::Matrix4d RobotTCPMatricx,Eigen::Matrix4d EndToolTCPMatricx);//例如一起执行插补
    //输入一个总的要到的目标位置，对应机器人ID和末端器械ID，求机器人要走到的目标位置和末端器械要走到的目标位置
    void MoveRCM(int robotid,int toolid,Eigen::Matrix4d Target_TCPMatricx,Eigen::Matrix4d &OUT_A,Eigen::Matrix4d &OUT_B);

    //实时交互方法：
    void TrackTEST_stop(int robotid,int toolid);
    void TrackTEST_hand(int robotid,int toolid,int handid,int followsource);//例如与segma主手交互
    void TrackTEST_camera(int robotid,int toolid,int cameraid);//例如与2D视觉交互
    void TrackTEST_ethercat();

    void run();

public slots:
    void runSomeBigWork1();


};





